/************************************************

     Brian Bousquet-Smith     Chaz Godwin
                
            OBDII Reader
                 Fall 2011
************************************************/

#include <avr/io.h>
#include <avr/iom164.h> 
#include <avr/interrupt.h>
#include <avr/delay.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>


/************************************************
             Defines
************************************************/

// States


#define OK         0
#define ELM_INIT_STATE    1
#define LCD_INIT_STATE    2
#define    OBD_INIT_STATE    3
#define DASHBOARD    4
#define PERFORMANCE    5
#define DIAGNOSTICS    6
#define ABOUT        7

// ELM Defines

#define COMMAND_END        0x0D

// OBD Defines

#define RESPONSE_END        0x3E



/************************************************
        Function Prototypes
************************************************/

// Initializing Functions

void power_on();
void serial_init();
int elm_init();
int obd_init();
int get_mph();

// Tx Rx Functions

void lcd_transmit(unsigned char data);
void obd_transmit(unsigned char data);
unsigned char lcd_receive();
unsigned char obd_receive();
void lcd_flush();
void obd_flush();
int obd_sendcmd(unsigned char *in, unsigned char *out, char len_in, char len_out);
int lcd_sendcmd(unsigned char *in, char len_in);

void print_bits(char byte);


/************************************************
        Global Variables
************************************************/

int state = ELM_INIT_STATE;
int timeoutflag = 0;

/************************************************
        Main Code
************************************************/




int main(void){

/*
*    Purpose: Output RPM and MPH data from a vehicle
*/
    power_on();
    serial_init();
    obd_flush();
    lcd_flush();
    int response;
    _delay_ms(5000);
    while(1){
        PORTA = state;
        switch(state){
        case ELM_INIT_STATE: // Initializes ELM
            lcd_transmit('1');
            response = elm_init();
            _delay_ms(1000);
            if(response == OK){
                state = OBD_INIT_STATE;
                break;
            }
            else{
                PORTA = 0x0F;
                break; 
            }

        case LCD_INIT_STATE: // Initializes LCD
            //response = lcd_init();
            if(response == OK){
                state = OBD_INIT_STATE;
                break;
            }
            else break;
    
        case OBD_INIT_STATE: // Initializes OBDII
            response = obd_init();
            if(response == OK){
                state = DASHBOARD;
                break;
            }
            else{
                PORTA = 0x07; 
                break;
            }

        case DASHBOARD: // Virtual Dashboard
            response = get_mph();
            _delay_ms(50);
            response = get_rpm();
            _delay_ms(50);
            break;
        
        case PERFORMANCE:
            break;        
        
        case DIAGNOSTICS:
            break;
        
        case ABOUT:
            break;
        } 
    }
    return 0;
}

void power_on(){

/*
*    Purpose: Turn the power light on of the device
*    Input: Nothing
*    Output: Nothing
*/

    DDRA=0xFF;
    PORTA = 0x0F;

}

void serial_init(){

/*
*    Purpose: Initialize Serial on Both UART Ports on the AVR
*    Input: Nothing
*    Output: Nothing
*/

    UCSR0A |= (1<<U2X0);            // Double speed for more precision
    UCSR0B |= (1<<RXEN0)|(1<<TXEN0);    // Allow transmit and receive
    UCSR0C |= (0<<USBS0)|(3<<UCSZ00);     // 8 Bit No Parity 1 Stop Bit
    UBRR0H = 0;                // Sets Baud Rate = 9600
    UBRR0L = 12;

    UCSR1A |= (1<<U2X1);            // Double speed for more precision
    UCSR1B |= (1<<RXEN1)|(1<<TXEN1);     // Allow transmit and receive
    UCSR1C |= (1<<USBS1)|(3<<UCSZ10);     // 8 Bit 2 Stop Bits
    UBRR1H = 0;                // Sets Baud Rate = 9600
    UBRR1L = 12;
}

int elm_init(){

/*
*    Purpose: Initialize ELM and check that communication is correct
*    Input: Nothing
*    Output:    Error Codes:
*        0 -    Successful
*        -1 -     Unsuccessful
*/
    unsigned char error_buff[10];
    unsigned char init_buff[3] = {'A', 'T', 'Z'};
    int i;        
    unsigned char exp_buff[22] = {'A', 'T', 'Z', 0x0D, 0x0A, 0x0D, 0x0A, 0x0D, 0x0A, 'E', 'L', 'M', '3', '2', '7', ' ', '1', '.', '4', 'b', 0x3E};
    unsigned char output_buff[22];
    unsigned char print_buff[9];    
    obd_sendcmd(init_buff, output_buff, 3, 22);
    for(i=0;i<sizeof(output_buff);i++){
        print_bits(output_buff[i]);
        lcd_transmit(' ');
        print_bits(exp_buff[i]);
        lcd_transmit('\n');    
        //if(output_buff[i]!=exp_buff[i]){
        //    //print_bits(output_buff[i]);
        //    sprintf(error_buff, "Error: %d\n", i);
        //    lcd_sendcmd(error_buff, sizeof(error_buff));
        //    return -1;
        //}
    }    
    sprintf(print_buff, "Welcome!\n");
    return 0;
    
}
    
int obd_init(){

/*
*    Purpose: Initialize OBDII port and check that communication is correct
*    Input: Nothing
*    Output: Error Codes:
*        0 -    Successful
*        -1 -     Unsuccessful
*/
    
    unsigned char init_buffer[4] = {'0', '1', '0', '0'};
    unsigned char obd_buffer[] = "SEARCHING...";
    int i;
    int flag;
    int count;    
    unsigned char echo;
    unsigned char response;
    
    // Figure out if OBD has valid protocol/is connected
    
    for(i=0;i<4;i++){
        obd_transmit(init_buffer[i]);
        echo = obd_receive();
        if (echo != init_buffer[i]) return -1;
    }
    obd_transmit(COMMAND_END);
    obd_receive(); // Get this echo
    obd_receive(); // Get new line feed     
    
    /* At this point OBD should response with 
        Searching...
           If nothing found outputs
            Unable to Connect
        If found protocol
            41 00 + 4 Bytes of Data 
    */
    flag = 0;
    count = 0;
        
    while( (response = obd_receive()) != RESPONSE_END){
        if(response == 'U') flag = -1;        
        lcd_transmit(response);
    }
    return flag;

}

/*int get_mph (void){

/*
*    Purpose: Get MPH Data and Output It
*    Input: Nothing
*    Output: Error Codes:
*        0 - Successful
*        -1 - Unsuccessful
*/

/*    unsigned char mph_buffer[4] = {'0', '1', '0', 'D'};    
    unsigned char exp_data[2] = {0x41, 0x0D};     
    unsigned char response_buffer[3];
    char mph;
    int i, error;
    
    error = obd_sendcmd(mph_buffer, response_buffer, 4, 3);
    if(error != 0) return -1;    
    for(i=0;i<sizeof(exp_data);i++){
        if(response_buffer[i] != exp_data) return -2);
    }
    mph = (unsigned char) response_buffer[2]*.621371;
    unsigned char output_buffer[8];
    sprintf(output_buffer, "MPH: %c", mph);
    int size;
    if(mph >100) size = 8;
    else if(mph < 10) size = 6;
    else size = 7;
    error = lcd_sendcmd(output_buffer, size);
    return error;
}     */


int get_mph(void){

/*
*    Purpose: Get RPM Data and Output it
*    Input: Nothing
*    Output: Error Codes:
*        0 - Successful
*        -1 - Unsuccessful
*/

    unsigned char rpm_buffer[4] = {'0', '1', '0', 'D'};
    unsigned char rpm_output[9] = {'R', 'P', 'M', ':', 0, 0, 0, 0, 0};
    unsigned char echo;
    unsigned char response;
    int i;
    
    //for(i=0;i<4;i++){
    //    obd_transmit(rpm_buffer[i]);
    //    echo = obd_receive();
    //    if (echo != rpm_buffer[i]) return -1;
    //}
    obd_transmit('0');
    obd_transmit('1');
    obd_transmit('0');
    obd_transmit('D');
    obd_transmit(0x0D);
    
    while( (response = obd_receive()) != RESPONSE_END){
        lcd_transmit(response);
    }
    return 0;
}


int get_rpm(void){

/*
*    Purpose: Get RPM Data and Output it
*    Input: Nothing
*    Output: Error Codes:
*        0 - Successful
*        -1 - Unsuccessful
*/

    unsigned char rpm_buffer[4] = {'0', '1', '0', 'C'};
    unsigned char rpm_output[9] = {'R', 'P', 'M', ':', 0, 0, 0, 0, 0};
    unsigned char echo;
    unsigned char response;
    int i;
    
    //for(i=0;i<4;i++){
    //    obd_transmit(rpm_buffer[i]);
    //    echo = obd_receive();
    //    if (echo != rpm_buffer[i]) return -1;
    //}
    obd_transmit('0');
    obd_transmit('1');
    obd_transmit('0');
    obd_transmit('C');
    obd_transmit(0x0D);
    
    while( (response = obd_receive()) != RESPONSE_END){
        lcd_transmit(response);
    }
    return 0;
}

int lcd_sendcmd(unsigned char *in, char len_in){

/*
*    Purpose: Send a string to the LCD to print
*    Input:    in - Input buffer to be printed
*        len-in - Length of the buffer to be printed
*    Output: Error Codes:
*        0 - Successful
*        -1 - Unsuccessful
*/    


    int i;
    //unsigned char input =
    for(i=0;i<len_in;i++){
        lcd_transmit(in[i]);        
    }    
    return 0;
}

int obd_sendcmd(unsigned char *in, unsigned char *out, char len_in, char len_out){
    //TODO Start Interrupt
    int i;    
    for(i=0;i<len_in;i++){
        obd_transmit(in[i]);
    }
    obd_transmit(0x0D);
    for(i=0;i<len_out;i++){
        out[i] = obd_receive();
    }    
    return 0;
}




void lcd_transmit (unsigned char data){
    
/*
*    Purpose: Transmit data to the LCD
*    Input: Data to be transmitted
*    Output:    Nothing
*/
    while( !(UCSR1A & (1<<UDRE1)));
    UDR1 = data;
}

unsigned char lcd_receive (void){

/*
*    Purpose: Receive data from the LCD
*    Input: Nothing
*    Output:    Data received    
*/

    while (!(UCSR1A & (1<<RXC1)));
    return UDR1;
}    

void lcd_flush(void){
    char tmp;
    while (UCSR1A & (1<<RXC1) ) tmp = UDR1;
}

void obd_transmit (unsigned char data){

/*
*    Purpose: Transmit data to the LCD
*    Input: Data to be transmitted
*    Output:    Nothing
*/

    while( !(UCSR0A & (1<<UDRE0)));
    UDR0 = data;
}

unsigned char obd_receive (void){

/*
*    Purpose: Receive data from the LCD
*    Input: Nothing
*    Output:    Data received    
*/
    while (!(UCSR0A & (1<<RXC0)));
        
    return UDR0;
}

void obd_flush(void) {
    char tmp;
    while( UCSR0A & (1<<RXC1)) tmp = UDR0;
}

/*char obd_sendcmd (char* in, char* out, char len_in, len_out){    
    //START CLOCK
    int i;    
    for(i=0;i<len_in;i++){
        obd_transmit(in+i);
    }
    for(i=0;i<len_out;i++){


}*/    

//char lcd_sendcmd (char* in, char* out, char len_in, len_out){

//}

void print_bits( char byte){
    char mask[8] = {
    0b10000000,
    0b01000000,
    0b00100000,
    0b00010000,
    0b00001000,
    0b00000100,
    0b00000010,
    0b00000001};

    int m;
    char tmp;
    for(m=0;m<8;m++){
        tmp = ((mask[m]&byte)>>(7-m))+48;
        lcd_transmit(tmp);
    }
}        








    
 